/*@author:yyx
*@date:2025-3-21
*@breif:码盘把位置，yaw角传给二车主控
*/


#include "AS5047.h"
#include "imu.h"
void AS5047_Init(AS5047_raw_t* AS_5047)
{
  AS_5047->address = 0;/*指针位置清零*/
  AS_5047->len = 20;/*规定数据帧长度*/
  
  AS_5047->AS5047_data.YAW = 0;
  AS_5047->AS5047_data.X_W_t = 0;
  AS_5047->AS5047_data.Y_W_t = 0;
}



/*数据同步操作，当数据帧同步成功则返回1，否则一直返回0*/
int SYNC(AS5047_raw_t* AS_5047,uint8_t rx_data)
{
  AS_5047->buff[0] =  AS_5047->buff[1];
  AS_5047->buff[1] =  rx_data;
  if(AS_5047->buff[0] == 0x20 && AS_5047->buff[1] == 0x25){ return 1; }
  return 0;
}

int check(AS5047_raw_t* AS_5047,uint8_t rx_data)
{
  if(AS_5047->address == 0)
  {
    if(SYNC(AS_5047,rx_data) != 1){return 0;}
      /*如果为1，已经同步成功*/
    AS_5047->address = 2;
  } 
  /*此时buff[0] = 0x20,buff[1] = 0x25*/
  AS_5047->buff[AS_5047->address] = rx_data;
  AS_5047->address++;
  
  
//  if(AS_5047->address == 4)/*数据帧的第三，四位为长度*/
//  {
//    /*检查数据帧的长度*/
//    if(AS_5047->len != AS_5047->buff[4])
//    {
//      AS_5047->address = 0;/*数据帧长度不对，重新发送*/
//      return -1;
//    }
//  }
  
  if(AS_5047->address == 20)/*一组完整的数据帧已经发送完毕,检查帧尾*/
  {
     if(AS_5047->buff[18] != 0x68 || AS_5047->buff[19] != 0x10)
     {
        AS_5047->address = 0;/*数据帧不对*/
        return -1;
     }
     AS_5047->address = 0;
  }
  return decode(AS_5047);
}

int decode(AS5047_raw_t* AS_5047)
{
     AS_5047->AS5047_data.YAW = R4(AS_5047->buff + 5);  
     AS_5047->AS5047_data.X_W_t = R4(AS_5047->buff + 9);
     AS_5047->AS5047_data.Y_W_t = R4(AS_5047->buff + 13);
     
     //AS_5047->address = 0;
     return 1;
}